package t111207;

import EDU.gatech.cc.is.util.Vec2;
import teams.ucmTeam.Behaviour;
import teams.ucmTeam.RobotAPI;

public class Defensa extends Behaviour{
	static double margen=0.45;
	@Override
	public void configure() {
	// No hacemos nada
	}
	@Override

	public int takeStep() {

		 double miX=myRobotAPI.getPosition().x;
		 double miY=myRobotAPI.getPosition().y;
 if(myRobotAPI.getPosition().x*-myRobotAPI.getFieldSide()<-0.75){
//	 Vec2 destino = new Vec2(0.5-myRobotAPI.getPosition().x,0.5-myRobotAPI.getPosition().y);
//	   myRobotAPI.setSteerHeading(destino.t);
//	  if(myRobotAPI.getPosition().x!=0){
//	   myRobotAPI.setSpeed(1.0);
//	  }
//	   else
//		  myRobotAPI.setSpeed(0);
// }*/

	 
	 if(myRobotAPI.getBall().r<0.4)
	 {		
		 myRobotAPI.setSteerHeading(myRobotAPI.getBall().t);
		 myRobotAPI.setSpeed(1.0);
		 //	myRobotAPI.setSteerHeading(myRobotAPI.getBall().t);
	 	 myRobotAPI.passBall(myRobotAPI.getClosestMate());
	 }
	 else 
	 { 
		 if(myRobotAPI.getPosition().y-myRobotAPI.getBall().y>0){
			 if(myRobotAPI.getPosition().y-myRobotAPI.getBall().y>0.38125){
			 Vec2 destino = new Vec2(myRobotAPI.getFieldSide()*1.15-miX,-0.3-miY);
			   myRobotAPI.setSteerHeading(destino.t);
			   if(detroDelMargen(miX, miY, destino.x, destino.y)){
				   orientarHaciaPelota();
				   myRobotAPI.setSpeed(0.0);
			   }
			   else
				   myRobotAPI.setSpeed(1.0);
			 }
			 else{
			  Vec2 destino = new Vec2(myRobotAPI.getFieldSide()*1.15-miX,-0.15-miY);
			   myRobotAPI.setSteerHeading(destino.t);
			   if(detroDelMargen(miX, miY, destino.x, destino.y)){
				   orientarHaciaPelota();
				   myRobotAPI.setSpeed(0.0);}
			   else
				   myRobotAPI.setSpeed(1.0);
			  
		   		}
		 	  
			  
		 	}
		 else{
			 
			 if(myRobotAPI.getPosition().y-myRobotAPI.getBall().y>-0.38125){
				 Vec2 destino = new Vec2(myRobotAPI.getFieldSide()*1.35-miX,0.15-miY);
				   myRobotAPI.setSteerHeading(destino.t);
				   if(detroDelMargen(miX, miY, destino.x, destino.y)){
					   orientarHaciaPelota();
					   myRobotAPI.setSpeed(0.0);
				   }
				   else
					   myRobotAPI.setSpeed(1.0);
				 }
			 else{
				  Vec2 destino = new Vec2(myRobotAPI.getFieldSide()*1.15-miX,0.3-miY);
				   myRobotAPI.setSteerHeading(destino.t);
				   if(detroDelMargen(miX, miY, destino.x, destino.y)){
					   orientarHaciaPelota();
					   myRobotAPI.setSpeed(0.0);
				   }
				   else
					   myRobotAPI.setSpeed(1.0);
				  
			   	}
			 
		 }
		 
	 
	 }
  }
  else  
	 	{	 Vec2 destino = new Vec2(myRobotAPI.getFieldSide()*0-miX,0-miY);
	  			myRobotAPI.setSteerHeading(destino.t);
	 			myRobotAPI.setSpeed(1.0);
	 	}	
	
			/*
			Vec2 dest = new Vec2(-1.145,0.5);
			myRobotAPI.setSteerHeading(dest.t);
			*/
	        
	  
		return RobotAPI.ROBOT_OK;
	}

	boolean detroDelMargen(double miX, double miY, double xDest, double yDest){
		if(miX<0){
			if(miY<0){ //Cuadrante inferior izquierdo
				return((miX>xDest-margen)&&(miY>yDest-margen));
			}
			else { //Cuadrante superior izquierdo
				return((miX>xDest-margen)&&(miY<yDest+margen));
			}
		}
		else{
			if(miY<0){ //Cuadrante inferior derecho
				return((miX<xDest+margen)&&(miY>yDest-margen));
			}
			else { //Cuadrante superior derecho
				return((miX<xDest+margen)&&(miY<yDest+margen));
			}
			
		}

	}
	public void orientarHaciaPelota(){
		 myRobotAPI.setSteerHeading(myRobotAPI.getBall().t);
	}
	@Override
	public void onInit(RobotAPI r) {
		r.setDisplayString("Defensa");
	}
	
	@Override
	public void end() {
		// No hacemos nada
	}
	
	@Override
	public void onRelease(RobotAPI r) {
		// No hacemos nada
	}
}